#include "FieldPrecomputer.cuh"

__constant__ float minimum_distance = 0.00001f;

__device__ void devCalMfield(float* magnet_pos, float* magnet_mm, float* pos, float* out, int numMagnet, float k) 
{

	int i;
	out[0] = 0; out[1] = 0;
	int last = numMagnet * 2;
	for (i=0; i<last; i+=2) {
		float nor[2];
		nor[0] = pos[0] - *(magnet_pos++);
		nor[1] = pos[1] - *(magnet_pos++);

		float dist = (nor[0] * nor[0] + nor[1] * nor[1]);
		dist = sqrtf(dist);
		if (dist < minimum_distance) continue;
		nor[0] /= dist;
		nor[1] /= dist;

		dist = powf(dist, -3);
		float factor = 3 * (nor[0] * magnet_mm[0] + nor[1] * magnet_mm[1]);
		out[0] += (nor[0] * factor - *(magnet_mm++)) * dist;
		out[1] += (nor[1] * factor - *(magnet_mm++)) * dist;
	}
	out[0] *= k;
	out[1] *= k;
}

__device__ void devCalForce(float* out, float* pos, float* mm, float* magnet_pos, float* magnet_mm, int numMagnet, float k) 
{
	int i;
	out[0] = 0; out[1] = 0;
	int last = numMagnet * 2;
	for (i=0; i<last; i+=2) {
		float nor[2];
		nor[0] = pos[0] - *(magnet_pos++);
		nor[1] = pos[1] - *(magnet_pos++);

		float dist = (nor[0] * nor[0] + nor[1] * nor[1]);
		dist = sqrtf(dist);
		if (dist < minimum_distance) continue;
		nor[0] /= dist;
		nor[1] /= dist;
		dist = powf(dist, -4) * 3;
		float mmidotnor = magnet_mm[0] * nor[0] + magnet_mm[1] * nor[1];
		float mmkdotnor = mm[0] * nor[0] + mm[1] * nor[1];
		float mmkdotmmi = magnet_mm[0] * mm[0] + magnet_mm[1] * mm[1];
		for (int j=0; j<2; j++) {
			float temp = -5 * nor[j] * mmkdotnor * mmidotnor;
			temp += nor[j] * mmkdotmmi;
			temp += mm[j] * mmidotnor;
			temp += magnet_mm[j] * mmkdotnor;
			out[j] +=  temp * dist;
		}
		magnet_mm+=2;
	}
	out[0] *= k;
	out[1] *= k;
}

__global__ void devSetMField (float* Mfield, FieldPrecomputerConstant data, float* magnet_pos, float* magnet_mm, int numMagnet, int numElements) {
	int tid = KERNEL_INDEX;
	if (tid < numElements) {
		float wpos[2];
		int fx = data.size_y;
		wpos[0] = data.magnetPos_x + (-data.max_x + tid/fx) / (float)data.scale;
		wpos[1] = data.magnetPos_y + (-data.max_y + tid%fx) / (float)data.scale;

		devCalMfield(magnet_pos, magnet_mm, wpos, Mfield + tid * 2, numMagnet, data.k);
	}

}

__global__ void devSetFfield(float* Ffield, const FieldPrecomputerConstant data, float* Mfield, float* magnet_pos, float* magnet_mm, int numMagnet, int numElements) {
	int tid = blockIdx.x * blockDim.x + threadIdx.x;
	if (tid < numElements) {

		float wpos[2];
		float mm[2];

		int fx = data.size_y;
		wpos[0] = data.magnetPos_x + (-data.max_x + tid/fx) / (float)data.scale;
		wpos[1] = data.magnetPos_y + (-data.max_y + tid%fx) / (float)data.scale;

		tid *= 2;
		float* mf = Mfield + tid;
		mm[0] = mf[0] * data.induceFactor;
		mm[1] = mf[1] * data.induceFactor;

		devCalForce(Ffield + tid, wpos, mm, magnet_pos, magnet_mm, numMagnet, data.k);
	}

}

void FieldPrecomputer::SetMField()
{

	KERNEL_CALL_DECLEAR(m_size);

	devSetMField<<<KERNEL_CALL>>> (dev_m_Mfield, data, dev_m_magnet_pos, dev_m_magnet_mm, m_magnet_num, m_size);
	
}

void FieldPrecomputer::SetFfield()
{
	KERNEL_CALL_DECLEAR(m_size);

	devSetFfield<<<KERNEL_CALL>>> (dev_m_Ffield, data, dev_m_Mfield, dev_m_magnet_pos, dev_m_magnet_mm, m_magnet_num, m_size);
}
